%clear; clc; close all;

load Q6_example_data_solution.mat

colors1 = ['km']; colors2 = ['bg'];
map_fig_id = figure; hold on; axis([-5 25 -5 25]); axis equal; xlabel('East'); ylabel('North');
spacing = 20;
for i= [1:spacing:size(xfilt,2) size(xfilt,2)]
    plot_uncertainty_ellipse(xfilt(1:3,i), Sigma_filt{i}, map_fig_id, colors1);
    for l=1:num_landmarks_discovered
       l_state_idxs = 3+ 2*(l-1)+1: 3 + 2*(l-1)+2;
       if(landmark_discovery_time(l) < i)
           % if we have seen the landmark a few times, start plotting it
           plot_uncertainty_ellipse(xfilt(l_state_idxs,i), Sigma_filt{i}(l_state_idxs,l_state_idxs), map_fig_id, colors2);
       end
    end
end

% plot final map
final_map_fig_id = figure; hold on; axis([-5 25 -5 25]); axis equal; title('map'); xlabel('East'); ylabel('North');
colors3 = ['kr'];
for l=1:num_landmarks_discovered
    l_state_idxs = 3+ 2*(l-1)+1: 3 + 2*(l-1)+2;
    %if(trace(Sigma_filt(l_state_idxs,l_state_idxs,i)) < 2*sigma0_landmarks^2 / 10)
        %% if we have seen the landmark a few times the variance will have decreased and we plot it
        plot_uncertainty_ellipse(xfilt(l_state_idxs,end), Sigma_filt{end}(l_state_idxs,l_state_idxs), final_map_fig_id, colors3);
    %end
end
axis equal;

